#ifndef MATRIX_H_
#define MATRIX_H_

#include <math.h>

#include "vector.h"

#define DEGTORAD(a)					((a) * ((float)M_PI/180.0f))
#define RADTODEG(a)					((a) * (180.0f/(float)M_PI))
#define SQR(x)						((x) * (x))

template <class TYPE>
class vector3;

template <class TYPE>
class matrix4 {

    friend class vector3<TYPE>;
public:
    TYPE   M11, M12, M13, M14,
           M21, M22, M23, M24,
           M31, M32, M33, M34,
           M41, M42, M43, M44;

public:
    matrix4() { reset(); }
	~matrix4() {}

	void reset(void)
	{
	    M11 = 1;    M12 = 0;    M13 = 0;    M14 = 0;
	    M21 = 0;    M22 = 1;    M23 = 0;    M24 = 0;
	    M31 = 0;    M32 = 0;    M33 = 1;    M34 = 0;
	    M41 = 0;    M42 = 0;    M43 = 0;    M44 = 1;
	}

	void printSelf()
	{
	    printf("%.3f\t%.3f\t%.3f\t%.3f\n", M11,M12,M13,M14);
	    printf("%.3f\t%.3f\t%.3f\t%.3f\n", M21,M22,M23,M24);
	    printf("%.3f\t%.3f\t%.3f\t%.3f\n", M31,M32,M33,M34);
	    printf("%.3f\t%.3f\t%.3f\t%.3f\n", M41,M42,M43,M44);
	}

	matrix4 operator * (const matrix4& rhs)
	{
		matrix4 mat;

	    mat.M11 = M11*rhs.M11 + M21*rhs.M12 + M31*rhs.M13 + M41*rhs.M14;
	    mat.M12 = M12*rhs.M11 + M22*rhs.M12 + M32*rhs.M13 + M42*rhs.M14;
	    mat.M13 = M13*rhs.M11 + M23*rhs.M12 + M33*rhs.M13 + M43*rhs.M14;
	    mat.M14 = M14*rhs.M11 + M24*rhs.M12 + M34*rhs.M13 + M44*rhs.M14;

	    mat.M21 = M11*rhs.M21 + M21*rhs.M22 + M31*rhs.M23 + M41*rhs.M24;
	    mat.M22 = M12*rhs.M21 + M22*rhs.M22 + M32*rhs.M23 + M42*rhs.M24;
	    mat.M23 = M13*rhs.M21 + M23*rhs.M22 + M33*rhs.M23 + M43*rhs.M24;
	    mat.M24 = M14*rhs.M21 + M24*rhs.M22 + M34*rhs.M23 + M44*rhs.M24;

	    mat.M31 = M11*rhs.M31 + M21*rhs.M32 + M31*rhs.M33 + M41*rhs.M34;
	    mat.M32 = M12*rhs.M31 + M22*rhs.M32 + M32*rhs.M33 + M42*rhs.M34;
	    mat.M33 = M13*rhs.M31 + M23*rhs.M32 + M33*rhs.M33 + M43*rhs.M34;
	    mat.M34 = M14*rhs.M31 + M24*rhs.M32 + M34*rhs.M33 + M44*rhs.M34;

	    mat.M41 = M11*rhs.M41 + M21*rhs.M42 + M31*rhs.M43 + M41*rhs.M44;
	    mat.M42 = M12*rhs.M41 + M22*rhs.M42 + M32*rhs.M43 + M42*rhs.M44;
	    mat.M43 = M13*rhs.M41 + M23*rhs.M42 + M33*rhs.M43 + M43*rhs.M44;
	    mat.M44 = M14*rhs.M41 + M24*rhs.M42 + M34*rhs.M43 + M44*rhs.M44;

	    return mat;
	}

	matrix4 operator* (const float scale)
	{
	    M11 *= scale;   M12 *= scale;   M13 *= scale;   M14 *= scale;
	    M21 *= scale;   M22 *= scale;   M23 *= scale;   M24 *= scale;
	    M31 *= scale;   M32 *= scale;   M33 *= scale;   M34 *= scale;
	    M41 *= scale;   M42 *= scale;   M43 *= scale;   M44 *= scale;
	    return *this;
	}

	// openGL set left-handed
	void setRotationLH_x(float angle_x)
	{
	   // reset();
	    M22 = cos(angle_x);
	    M23 = -sin(angle_x);
	    M32 = sin(angle_x);
	    M33 = cos(angle_x);
	}

	void setRotationLH_y(float angle_y)
	{
	    //reset();
	    M11 = cos(-angle_y);
	    M13 = sin(-angle_y);
	    M31 = -sin(-angle_y);
	    M33 = cos(-angle_y);
	}

	void setRotationLH_z(float angle_z)
	{
	  //  reset();
	    M11 = cos(angle_z);
	    M12 = -sin(angle_z);
	    M21 = sin(angle_z);
	    M22 = cos(angle_z);
	}

	// directX set right-handed
	void setRotationRH_x(float angle_x)
	{
	  //  reset();
	    M22 = cos(angle_x);
	    M23 = sin(angle_x);
	    M32 = -sin(angle_x);
	    M33 = cos(angle_x);
	}

	void setRotationRH_y(float angle_y)
	{
	 //   reset();
	    M11 = cos(angle_y);
	    M13 = -sin(angle_y);
	    M31 = sin(angle_y);
	    M33 = cos(angle_y);
	}

	void setRotationRH_z(float angle_z)
	{
	 //   reset();
	    M11 = cos(angle_z);
	    M12 = sin(angle_z);
	    M21 = -sin(angle_z);
	    M22 = cos(angle_z);
	}

	void setRotation_LH(vector3<TYPE> &rhs)
	{
	    setRotation_LH(rhs.X,rhs.Y,rhs.Z);
	}

	void setRotation_LH(float x, float y, float z)
	{
	  //  reset();

	    float cx = cos(x), cy = cos(y), cz = cos(z);
	    float sx = sin(x), sy = sin(y), sz = sin(z);

	    float cxsy = cx*sy;
	    float sxsy = sx*sy;

	    M11 = cx*cy;    M12 = cxsy*sz - sx*cz;    M13 = cxsy*cz + sx*sz;
	    M21 = sx*cy;    M22 = sxsy*sz + cx*cz;    M23 = sxsy*cz - cx*sz;
	    M31 = -sy;      M32 = cy*sz;              M33 = cy*cz;
	}

	void setRotation_RH(vector3<TYPE> &rhs)
	{
	    setRotation_RH(rhs.X,rhs.Y,rhs.Z);
	}

	void setRotation_RH(float x, float y, float z)
	{
	  //  reset();

	    float cx = cos(x), cy = cos(y), cz = cos(z);
	    float sx = sin(x), sy = sin(y), sz = sin(z);

	    float sxsy = sx*sy;
	    float cxsy = cx*sy;

	    M11 = cy*cz;                M12 = cy * sz;             M13 = -sy;
	    M21 = sxsy*cz - cx*sz;      M22 = sxsy*sz + cx*cz;     M23 = sx*cy;
	    M31 = cxsy*cz + sx*sz;      M32 = cxsy*sz - sx*cz;     M33 = cx*cy;
	}

	void Transpose(void)
	{
	    matrix4 tmp_mat;
	    tmp_mat = *this;

	    M11 = tmp_mat.M11;	    M12 = tmp_mat.M21;	    M13 = tmp_mat.M31;	    M14 = tmp_mat.M41;
	    M21 = tmp_mat.M12;	    M22 = tmp_mat.M22;	    M23 = tmp_mat.M32;	    M24 = tmp_mat.M42;
	    M31 = tmp_mat.M13;	    M32 = tmp_mat.M23;	    M33 = tmp_mat.M33;	    M34 = tmp_mat.M43;
	    M41 = tmp_mat.M14;	    M42 = tmp_mat.M24;	    M43 = tmp_mat.M34;	    M44 = tmp_mat.M44;
	}

	matrix4& operator = (const matrix4& rhs)
	{
		if (this == &rhs)
			return *this;

	    M11 = rhs.M11;	    M12 = rhs.M12;	    M13 = rhs.M13;	    M14 = rhs.M14;
	    M21 = rhs.M21;	    M22 = rhs.M22;	    M23 = rhs.M23;	    M24 = rhs.M24;
	    M31 = rhs.M31;	    M32 = rhs.M32;	    M33 = rhs.M33;	    M34 = rhs.M34;
	    M41 = rhs.M41;	    M42 = rhs.M42;	    M43 = rhs.M43;	    M44 = rhs.M44;

		return *this;
	}

	float getDeterminant(void)
	{
	    float value =
	        (M11*M22-M12*M21) * (M33*M44-M34*M43) -	(M11*M23-M13*M21) * (M32*M44-M34*M42) +
			(M11*M24-M14*M21) * (M32*M43-M33*M42) +	(M12*M23-M13*M22) * (M31*M44-M34*M41) -
			(M12*M24-M14*M22) * (M31*M43-M33*M41) +	(M13*M24-M14*M23) * (M31*M42-M32*M41);

	    return value;
	}

	void invert(void)
	{
	    matrix4 m_tmp;

	    m_tmp.M11 = M22 * (M33 * M44 - M34 * M43) + M23 * (M34 * M42 - M32 * M44) + M24 * (M32 * M43 - M33 * M42);
	    m_tmp.M12 = M32 * (M13 * M44 - M14 * M43) + M33 * (M14 * M42 - M12 * M44) + M34 * (M12 * M43 - M13 * M42);
	    m_tmp.M13 = M42 * (M13 * M24 - M14 * M23) + M43 * (M14 * M22 - M12 * M24) + M44 * (M12 * M23 - M13 * M22);
	    m_tmp.M14 = M12 * (M24 * M33 - M23 * M34) + M13 * (M22 * M34 - M24 * M32) + M14 * (M23 * M32 - M22 * M33);

	    m_tmp.M21 = M23 * (M31 * M44 - M34 * M41) + M24 * (M33 * M41 - M31 * M43) + M21 * (M34 * M43 - M33 * M44);
	    m_tmp.M22 = M33 * (M11 * M44 - M14 * M41) + M34 * (M13 * M41 - M11 * M43) + M31 * (M14 * M43 - M13 * M44);
	    m_tmp.M23 = M43 * (M11 * M24 - M14 * M21) + M44 * (M13 * M21 - M11 * M23) + M41 * (M14 * M23 - M13 * M24);
	    m_tmp.M24 = M13 * (M24 * M31 - M21 * M34) + M14 * (M21 * M33 - M23 * M31) + M11 * (M23 * M34 - M24 * M33);

	    m_tmp.M31 = M24 * (M31 * M42 - M32 * M41) + M21 * (M32 * M44 - M34 * M42) + M22 * (M34 * M41 - M31 * M44);
	    m_tmp.M32 = M34 * (M11 * M42 - M12 * M41) + M31 * (M12 * M44 - M14 * M42) + M32 * (M14 * M41 - M11 * M44);
	    m_tmp.M33 = M44 * (M11 * M22 - M12 * M21) + M41 * (M12 * M24 - M14 * M22) + M42 * (M14 * M21 - M11 * M24);
	    m_tmp.M34 = M14 * (M22 * M31 - M21 * M32) + M11 * (M24 * M32 - M22 * M34) + M12 * (M21 * M34 - M24 * M31);

	    m_tmp.M41 = M21 * (M33 * M42 - M32 * M43) + M22 * (M31 * M43 - M33 * M41) + M23 * (M32 * M41 - M31 * M42);
	    m_tmp.M42 = M31 * (M13 * M42 - M12 * M43) + M32 * (M11 * M43 - M13 * M41) + M33 * (M12 * M41 - M11 * M42);
	    m_tmp.M43 = M41 * (M13 * M22 - M12 * M23) + M42 * (M11 * M23 - M13 * M21) + M43 * (M12 * M21 - M11 * M22);
	    m_tmp.M44 = M11 * (M22 * M33 - M23 * M32) + M12 * (M23 * M31 - M21 * M33) + M13 * (M21 * M32 - M22 * M31);

	    float d = getDeterminant();
	    if(d) m_tmp =  m_tmp * (1/d);

	    *this = m_tmp;
	}

	void setTranslation(vector3<TYPE> &pos)
	{
	    setTranslation(pos.X, pos.Y, pos.Z);
	}

	void setTranslation(TYPE x, TYPE y, TYPE z)
	{
	    M41 = x;
	    M42 = y;
	    M43 = z;
	}
/*
	void printSelf(void)
	{
		std::cout << M11 << "\t\t" << M12 << "\t\t" << M13 << "\t\t" << M14 << std::endl;
		std::cout << M21 << "\t\t" << M22 << "\t\t" << M23 << "\t\t" << M24 << std::endl;
		std::cout << M31 << "\t\t" << M32 << "\t\t" << M33 << "\t\t" << M34 << std::endl;
		std::cout << M41 << "\t\t" << M42 << "\t\t" << M43 << "\t\t" << M44 << std::endl;
	}*/
};

typedef matrix4<float> matrix4f;

#endif // MATRIX_H_

